#ifndef CPLO_CPP
#define CPLO_CPP

#include "cplo.h"
#include "cplo_principal.cpp"
#include <cstring>
#include <cfloat>

#ifdef MAP_HEIGHT
#ifdef MAP_WIDTH
#ifdef N_OBJETOS

#define INFINITO 1E+20


cplo::cplo() {
    nroObjetos = 0;
    nroAtrativas = 0;
    nroRepulsivas = 0;
    memset (objeto, 0, sizeof(objeto));
    memset (potencialX, 0, sizeof(potencialX));
    memset (potencialY, 0, sizeof(potencialY));
    cteAtrativa = mapa[0][0].getCteAtrativa();
    cteRepulsiva = mapa[0][0].getCteRepulsiva();
    cteRepulsivaFixa = mapa[0][0].getCteRepulsivaFixa();
    RadiusMax = 100;
}

void cplo::reset() {
    for (int i = 0; i < MAP_HEIGHT; i++)
        for (int j = 0; j < MAP_WIDTH; j++)
            mapa[i][j].reset();
    nroObjetos = 0;
    nroAtrativas = 0;
    nroRepulsivas = 0;
    memset (objeto, 0, sizeof(objeto));
    cteAtrativa = mapa[0][0].getCteAtrativa();
    cteRepulsiva = mapa[0][0].getCteRepulsiva();
    cteRepulsivaFixa = mapa[0][0].getCteRepulsivaFixa();
    at_x.clear();
    at_y.clear();
    rep_x.clear();
    rep_y.clear();
    RadiusMax = 100;
    }

double cplo::getPotencialX(int px, int py) {
    if (fabs(mapa[px][py].getAnguloResultante()) <= PI)
        return mapa[px][py].getResultanteX;
    else
        return (-1)*(mapa[px][py].getResultanteX);
    }

double cplo::getPotencialY(int px, int py) {
    double tmp = mapa[px][py].getAnguloResultante();
    if (tmp >= 0 && tmp <= PI)
        return mapa[px][py].getResultanteY;
    else
        return (-1)*(mapa[px][py].getResultanteY);
    }

void cplo::addAtrativa(int px, int py) {
    at_x.push_back(px);
    at_y.push_back(py);
}

void cplo::addRepulsiva(int px, int py) {
    rep_x.push_back(px);
    rep_y.push_back(py);
}

void cplo::addRepulsivaFixa(int px, int py) {
    repFixa_x.push_back(px);
    repFixa_y.push_back(py);
}

void cplo::setCteAtrativa(double d) {
    cteAtrativa = d;
    }

void cplo::setCteRepulsiva(double d) {
    cteRepulsiva = d;
    }

void cplo::setCteRepulsivaFixa(double d) {
    cteRepulsivaFixa = d;
    }

void cplo::setObjeto(int px, int py) {
    objeto[nroObjetos][0] = px;
    objeto[nroObjetos++][1] = py;
    }

double cplo::getCteAtrativa() {
    return cteAtrativa;
    }

double cplo::getCteRepulsiva() {
    return cteRepulsiva;
}

double cplo::getCteRepulsivaFixa) {
    return cteRepulsivaFixa;
}

Vetor<double> cplo::getVetorResultante(int px, int py) {
    return mapa[px][py].getVetorResultante();
    }

double cplo::getAnguloResultante(int px, int py) {
    return mapa[px][py].getAnguloResultante();
    }

void cplo::getNeighborhood(int nx, int ny) { //VERIFICAR SISTEMA DE COORDENADAS
    if (nx == 0) {
        vizinhos.botton[0] = vizinhos.botton[1]= INFINITO;
        vizinhos.top[0] = getPotentialX(nx+1, ny);
        vizinhos.top[1] = getPotentialY(nx+1, ny);
    } else if (nx+1 == MAP_WIDTH) {
        vizinhos.top[0] = vizinhos.top[1]= INFINITO;
        vizinhos.botton[0] = getPotentialX(nx-1, ny);
        vizinhos.botton[1] = getPotentialY(nx-1, ny);
    } else {
        vizinhos.botton[0] = getPotentialX(nx-1, ny);
        vizinhos.botton[1] = getPotentialY(nx-1, ny);
        vizinhos.top[0] = getPotentialX(nx+1, ny);
        vizinhos.top[1] = getPotentialY(nx+1, ny);
        }

    if (ny == 0) {
        vizinhos.left[0] = vizinhos.left[1]= INFINITO;
        vizinhos.right[0] = getPotentialX(nx, ny+1);
        vizinhos.right[1] = getPotentialY(nx, ny+1);
    } else if (ny+1 == MAP_WIDTH) {
        vizinhos.right[0] = vizinhos.right[1]= INFINITO;
        vizinhos.left[0] = getPotentialX(nx, ny-1);
        vizinhos.left[1] = getPotentialY(nx, ny-1);
    } else {
        vizinhos.left[0] = getPotentialX(nx, ny-1);
        vizinhos.left[1] = getPotentialY(nx, ny-1);
        vizinhos.right[0] = getPotentialX(nx, ny+1);
        vizinhos.right[1] = getPotentialY(nx, ny+1);
        }
    }

int cplo::getRobotListSize() {
    return nroObjetos;
    }

void cplo::setRadiusOfInfluence(double v) {
    RadiusMax = v;
    }

double cplo::getRadiusOfInfluence() {
    return RadiusMax;
    }

void cplo::setEpsilon(double v) {
    epsilon = v;
    }

double cplo::getEpsilon() {
    return epsilon;
    }


#endif
#endif
#endif
#endif
